/*
  File:    GaussianFilter.cpp
  Purpose: Filters data with a Gaussian filter. 

  Contact: Paul Macklin
           pmacklin@math.uci.edu
		  http://math.uci.edu/~pmacklin
*/

#include <cmath>
#include <iostream>
#include <cstdlib>
#include <cstdio>

using namespace std;

#include "../ImprovedMath.h"
#include "../Matrix.h"
#include "extend.h"

double KernelMatrixProduct( Matrix& kernel, Matrix& Input, int Row, int Col, char Direction )
{
 int i;
 int N = (kernel.TellCols() - 1)/2;
 
 if( Direction == 'H' || Direction == 'h' )
 {
  double output = 0.0;
  for( i = 0 ; i < kernel.TellCols() ; i++ )
  { output += ( *kernel(i) * *Input( Row, Col-N+i ) ); }
  return output;
 }
 if( Direction == 'V' || Direction == 'v' )
 {
  double output = 0.0;
  for( i = 0 ; i < kernel.TellCols() ; i++ )
  { output += ( *kernel(i) * *Input( Row-N+i, Col ) ); }
  return output;
 }
 return 0.0;
}

Matrix GaussianFilter( Matrix& Xin, Matrix& Yin, Matrix &Input, double StandardDeviation )
{
 double Sqrt2Pi = 2.50662827463100;

 double dX = *Xin(1) - *Xin(0);
// cout<<"after Xin"<<endl;
// getchar();
 double dY = *Yin(1) - *Yin(0);
// cout<<"after Yin"<<endl;
// getchar();
 Matrix GaussianKernel;
 int N = (int) ceil( 3.0*StandardDeviation / dX ); 
 GaussianKernel.SetSize( 1, 1 + 2*N );
 
 int i,j;
 double sum = 0.0;
 for( i=0 ; i < 2*N+1 ; i++ )
 {
  double x = (i-N)*dX;
  *GaussianKernel(i) =  exp( -0.5 * square( x / StandardDeviation ) );
  sum += *GaussianKernel(i);
 }
 GaussianKernel = (1.0/sum) * GaussianKernel;

 Matrix Output = zeros( Input.TellRows() , Input.TellCols() );

 
 Matrix Extended = extend( Input, N );


 // smooth the x-direction;
 for( j=N ; j < Extended.TellCols() - N ; j++ )
 {
  for( i=N ; i < Extended.TellRows() - N ; i++ )
  {
   *Output(i-N,j-N) = KernelMatrixProduct( GaussianKernel,Extended, i,j, 'H' );
  }
 }
 
 N = (int) ceil( 3.0*StandardDeviation / dY ); 
 GaussianKernel.SetSize( 1, 1 + 2*N );
 
 sum = 0.0;
 for( i=0 ; i < 2*N+1 ; i++ )
 {
  double y = (i-N)*dY;
  *GaussianKernel(i) =  exp( -0.5 * square( y / StandardDeviation ) );
  sum += *GaussianKernel(i);
 }
 GaussianKernel = (1.0/sum) * GaussianKernel;

 Extended = extend( Output, N );
 // smooth the y-direction;
 for( j=N ; j < Extended.TellCols() - N ; j++ )
 {
  for( i=N ; i < Extended.TellRows() - N ; i++ )
  {
   *Output(i-N,j-N) = KernelMatrixProduct( GaussianKernel,Extended, i,j, 'V' );
  }
 }
 
 
 return Output;
}

Matrix GaussianFilterWithReflection( Matrix& Xin, Matrix& Yin, Matrix &Input, double StandardDeviation )
{
 double Sqrt2Pi = 2.50662827463100;

 double dX = *Xin(1) - *Xin(0);
 double dY = *Yin(1) - *Yin(0);
 
 Matrix GaussianKernel;
 int N = (int) ceil( 3.0*StandardDeviation / dX ); 
 GaussianKernel.SetSize( 1, 1 + 2*N );
 
 int i,j;
 double sum = 0.0;
 for( i=0 ; i < 2*N+1 ; i++ )
 {
  double x = (i-N)*dX;
  *GaussianKernel(i) =  exp( -0.5 * square( x / StandardDeviation ) );
  sum += *GaussianKernel(i);
 }
 GaussianKernel = (1.0/sum) * GaussianKernel;

 Matrix Output = zeros( Input.TellRows() , Input.TellCols() );
 
 Matrix Extended = zeros( Input.TellRows() + 2*N , Input.TellCols() + 2*N );
 
 for( i=0 ; i < Input.TellRows() ; i++ )
 {
  for( j = 0 ; j < Input.TellCols() ; j++ )
  {
   *Extended(N+i,N+j) = *Input(i,j);
  }
 }
 
 for( i=0; i < N ; i++ )
 {
  for( j=N ; j < N + Input.TellCols() ; j++ )
  {
   *Extended(i,j) = *Extended(2*N-i-1,j);
   *Extended(N+Input.TellRows()+i,j) = *Extended(N+Input.TellRows()-1-i,j);
  }
 }

 for( i = 0 ; i < Output.TellRows() ; i++ )
 {
  for( j=0 ; j < N ; j++ )
  {
   *Extended(i,j) = *Extended(i,2*N-j-1);
   *Extended(i,N+Input.TellCols()+j) = *Extended(i,N+Input.TellCols()-1-j);
  }
 }

 // smooth the x-direction;
 for( j=N ; j < Extended.TellCols() - N ; j++ )
 {
  for( i=N ; i < Extended.TellRows() - N ; i++ )
  {
   *Output(i-N,j-N) = KernelMatrixProduct( GaussianKernel,Extended, i,j, 'H' );
  }
 }
 
 N = (int) ceil( 3.0*StandardDeviation / dY ); 
 GaussianKernel.SetSize( 1, 1 + 2*N );
 
 sum = 0.0;
 for( i=0 ; i < 2*N+1 ; i++ )
 {
  double y = (i-N)*dY;
  *GaussianKernel(i) =  exp( -0.5 * square( y / StandardDeviation ) );
  sum += *GaussianKernel(i);
 }
 GaussianKernel = (1.0/sum) * GaussianKernel;

 Extended = extend( Output, N );
 // smooth the y-direction;
 for( j=N ; j < Extended.TellCols() - N ; j++ )
 {
  for( i=N ; i < Extended.TellRows() - N ; i++ )
  {
   *Output(i-N,j-N) = KernelMatrixProduct( GaussianKernel,Extended, i,j, 'V' );
  }
 }
 
 
 return Output;
}


Matrix GaussianFilterNonExtended( Matrix& Xin, Matrix& Yin, Matrix &Input, double StandardDeviation )
{
 double Sqrt2Pi = 2.50662827463100;

 double dX = *Xin(1) - *Xin(0);
 double dY = *Yin(1) - *Yin(0);
 
 Matrix GaussianKernel;
 int N = (int) ceil( 3.0*StandardDeviation / dX ); 
 GaussianKernel.SetSize( 1, 1 + 2*N );

 int i,j;
 double sum = 0.0;
 for( i=0 ; i < 2*N+1 ; i++ )
 {
  double x = (i-N)*dX;
  *GaussianKernel(i) =  exp( -0.5 * square( x / StandardDeviation ) );
  sum += *GaussianKernel(i);
 }
 GaussianKernel = (1.0/sum) * GaussianKernel;

 
 Matrix Output = zeros( Input.TellRows() , Input.TellCols() );
 Matrix Extended = Input; 
 
 // smooth the x-direction;
 for( j=N ; j < Extended.TellCols()-N ; j++ )
 {
  for( i=N ; i < Extended.TellRows() - N ; i++ )
  {
   *Output(i,j) = KernelMatrixProduct( GaussianKernel,Extended, i,j, 'H' );
  }
 }
 
 N = (int) ceil( 3.0*StandardDeviation / dY ); 
 GaussianKernel.SetSize( 1, 1 + 2*N );
 
 sum = 0.0;
 for( i=0 ; i < 2*N+1 ; i++ )
 {
  double y = (i-N)*dY;
  *GaussianKernel(i) =  exp( -0.5 * square( y / StandardDeviation ) );
  sum += *GaussianKernel(i);
 }
 GaussianKernel = (1.0/sum) * GaussianKernel;

 
 Extended = Output;
// Extended = Output; // extend( Output, N );

 // smooth the y-direction;
 for( j=N ; j < Extended.TellCols() - N ; j++ )
 {
  for( i=N ; i < Extended.TellRows()-N ; i++ )
  {
   *Output(i,j) = KernelMatrixProduct( GaussianKernel,Extended, i,j, 'V' );
  }
 }
 
 
 return Output;
}
